/*
 *  BCControlArchitecture.cpp
 *  roborobo-online
 *
 *  Created by Antonio Gonzalez on 29/04/10.
 *
 */

#include "BirthClinic/include/BCControlArchitecture.h"
#include "BirthClinic/include/BirthClinic.h"
#include "BirthClinic/include/BCAgentWorldModel.h"
#include "BirthClinic/include/BCSharedData.h"
#include "BirthClinic/include/Agents/BCAgent.h"
#include "Behavior/WallAvoidanceBehavior.h"
#include "World/World.h"

BCControlArchitecture::BCControlArchitecture(RobotAgentWorldModel *__wm) :
		BehaviorControlArchitecture(__wm) {
	// nothing to do
}

BCControlArchitecture::~BCControlArchitecture() {
	// nothing to do.
}

void BCControlArchitecture::reset() {
	// nothing to do.
}

void BCControlArchitecture::step() {

	BCAgentWorldModel* worldModel = dynamic_cast<BCAgentWorldModel*>(_wm);
	BCAgentPtr agent = boost::dynamic_pointer_cast<BCAgent>(
			gWorld->getAgentById(worldModel->_agentId));

	if (agent->isMainAgent()) {

		int constructionPeriod = agent->getConstructionPeriod();

		if(constructionPeriod > 0){
			agent->increaseAge();
			return;
		}

		LifeCycleState state = agent->getState();

		switch (state) {
		case ADULT: {
			WallAvoidanceBehavior::step(_wm);
			agent->increaseAge();
		}
			break;

		case KID: {
			// If the robot is a kid, it follows the initial random direction.
			// If it finds an obstacle (wall or robot), it change the direction and follows this new one.

			double speed =
					+1
							- ((double) gSensorRange
									- ((_wm->getDefaultSensors()->getSensorDistanceValue(
											2)
											+ _wm->getDefaultSensors()->getSensorDistanceValue(
													3)) / 2))
									/ (double) gSensorRange;

			_wm->_desiredTranslationalValue = speed;

			std::vector<int> components = agent->getComponents();
			std::vector<int> totalSensorValues;

			int size = components.size();

			for(int i = 0; i<size; i++){
				int compId = components.at(i);
				BCAgentPtr comp = boost::dynamic_pointer_cast<BCAgent>(
							gWorld->getAgentById(compId));

				int westSensorValue = comp->getSensorValue(0);
				int northSensorValue = comp->getSensorValue(1);
				int eastSensorValue = comp->getSensorValue(2);
				int southSensorValue = comp->getSensorValue(3);

				if(i==0){
					totalSensorValues.push_back(westSensorValue);
					totalSensorValues.push_back(northSensorValue);
					totalSensorValues.push_back(eastSensorValue);
					totalSensorValues.push_back(southSensorValue);
				}
				else{
					totalSensorValues[0] += westSensorValue;
					totalSensorValues[1] += northSensorValue;
					totalSensorValues[2] += eastSensorValue;
					totalSensorValues[3] += southSensorValue;
				}
			}

			totalSensorValues[0] /= size;
			totalSensorValues[1] /= size;
			totalSensorValues[2] /= size;
			totalSensorValues[3] /= size;


			if (_wm->getDefaultSensors()->getSensorDistanceValue(0)
					+ _wm->getDefaultSensors()->getSensorDistanceValue(1)
					+ _wm->getDefaultSensors()->getSensorDistanceValue(2)
					< _wm->getDefaultSensors()->getSensorDistanceValue(3)
							+ _wm->getDefaultSensors()->getSensorDistanceValue(
									4)
							+ _wm->getDefaultSensors()->getSensorDistanceValue(
									5)) {
				_wm->_desiredRotationalVelocity = +5;
			} else if (_wm->getDefaultSensors()->getSensorDistanceValue(3)
					+ _wm->getDefaultSensors()->getSensorDistanceValue(4)
					+ _wm->getDefaultSensors()->getSensorDistanceValue(5)
					< 3 * gSensorRange) {
				_wm->_desiredRotationalVelocity = -5;
			} else if (_wm->_desiredRotationalVelocity > 0) {
				_wm->_desiredRotationalVelocity--;
			} else if (_wm->_desiredRotationalVelocity < 0) {
				_wm->_desiredRotationalVelocity++;
			} else {
				_wm->_desiredRotationalVelocity = 0;
			}

			agent->increaseAge();
		}
			break;

		case DEAD: {
			// If the organism die, it is removed from the simulator.
			agent->unregisterAgent();

			int numComponents = agent->getOrganism()->agents.size();

			for (int i = numComponents - 1; i > 0; i--) {
				BCAgentPtr ag = boost::dynamic_pointer_cast<BCAgent>(
						agent->getOrganism()->agents.at(i));

				ag->unregisterAgent();

				int agId = ag->getId();
				agent->getOrganism()->removeRobot(ag);
				ag->setOrganism(NULL);

				gWorld->removeAgent(agId);
			}

			if (agent->getOrganism() != NULL)
				agent->getOrganism()->setInactive();

			agent->setOrganism(NULL);

			gWorld->removeAgent(worldModel->_agentId);
		}

		}
	}
}
